% !Mode:: "TeX:UTF-8" 

\BiChapter{基于概率拓扑的协同定位构型规划方法}{Active and cooperative localization using path planning}

\BiSection{引言}{Introduction}
精准的自定位功能是多智能体系统实现众多协同自主任务的基础技术之一。在日常生活中，GNSS可以为大部分的应用提供精准且易获取的定位导航信号。然而在GNSS拒止的环境中，移动平台只能通过搭载各类传感器，对环境和自身的运动信息进行感知，然后通过融合自身各类传感器的感知数据，实现自定位的功能。不过由于运动过程中受到的外部扰动和传感器观测过程中存在的随机噪声的影响，节点通常无法获得精确的定位信息。因此如何增加定位信息来源，减少定位过程中的不确定性因素，增加定位精度，成为了多智能体系统自定位过程的主要任务。

多智能体系统的协同定位方法是通过在每个节点上搭载相对观测传感器和通信设备，从而在任务中通过协作的方式（如通过执行相对观测和数据共享），额外地增加定位信息来源，从而优化定位精度的方法。由于协同定位框架中的观测数据是通过协作的方式产生的，而不是依赖于对环境的感知，因此协同定位方法在高对抗、大尺度、具备单一特征、或动态特征的环境中具有优势。

由于多智能体系统都是由合作节点组成的，因此针对特定问题设计优化的协同策略可以大幅度提升任务性能。优化的协同策略包括如何更加合理地利用协同测量数据，如何获得最佳的相对观测构型配置以及如何处理信息之间的耦合等方面。主动协同定位方法是指在定位过程中主动地去预测系统在未来时刻的性能表现，从而选择最佳的系统配置，以优化多智能体系统内部对定位过程可能产生影响的各类因素，使相对测量过程更为合理，测量数据更加可靠，定位精度更高。

本章研究一类基于路径规划的主动协同定位方法。已有的研究成果表明，当系统可以获取绝对定位信息时（例如系统中至少有一个节点存在GNSS测量信息，或者可以观测到一个固定的锚节点），则该系统的协同定位过程是可观测的，因此系统中所有节点的稳态定位误差是有界的。该界限与节点初始的位置误差无关，而是受到系统内部的相对测量拓扑和节点装配的对内/对外测量传感器的精度的影响。因此作为一个可控变量，系统每一时刻的相对观测拓扑可以被用来优化协同定位的性能。另外，当系统内每个智能体装备的测量和通信设备的最大观测和通信距离受限时，系统内部的通信观测拓扑将不再是固定不变的，而是会随着节点之间相对位置的变化而改变。又由于各个智能体都存在自定位误差，因此主动协同定位过程对未来时刻系统内部通信测量拓扑的预测将是随机的、不确定的。

基于以上问题，本章首先基于优化理论，将每个智能体的运动轨迹视作优化变量，控制多智能体内部相互观测构型，以获取最佳定位数据，通过求取最优的运动路径，提升协同定位任务的性能。其次，本章基于前一章的研究基础，引入概率拓扑的概念，即将系统在未来时刻的通信和观测拓扑视作随机和不确定的，从而设计了一类基于概率拓扑的主动协同定位方法。该方法通过前一章设计的未来连通概率预测算法，计算未来时刻每一种可能的观测拓扑出现的概率，从而预测协同定位任务在给定路径上性能表现的期望值。以此来评价每条路径的优先值，从而规划最佳的路径输入。

\BiSection{信念空间阐述}{arg2}
本节对信念空间的概念进行阐述，并总结信念空间规划问题的通用求解框架。

\BiSubsection{信念系统与估计问题}{arg2}

信念空间是状态空间所有可能的随机分布，它不仅包含所有状态，还包含状态由于系统不确定性而的概率密度分布。
本节对具有$N$个跟随者和$M$个领导者组成的多智能体系统进行信念空间建模。用$k$表示当前时刻，$\vect{p}_i(k)$为当前时刻第$i$个智能体的真实位置，$\vect{W}_j$表示第$j$个锚节点的状态，参考前一章，单个节点的状态转移模型为：
\begin{equation} \label{eq. ACOTO_motionModel1}
	\vect{p}_i(k+1) = f_i \left[ \vect{p}_i(k),\vect{u}_i(k),\vect{w}_i \right],
\end{equation}
可将上式转变为概率形式$p(\vect{p}_i(k+1) | \vect{p}_i(k), \vect{u}_i(k) )$
参考前一章对观测的定义$\vect{z}_{ij}^k = \{ \vect{\vec{z}}_{ij}^k , s_{ij}^k \}$，实际观测数据的观测模型为：
\begin{equation} \label{eq. ACOTO_observationModel1}
	\vect{\vec{z}}_{i,j}(k) = h_{i,j} \left[ \vect{p}_i(k),\vect{X}_j(k), \vect{v}_{i,j} \right],
\end{equation}
记上述实际观测数据的的概率形式为$p( \vect{\vec{z}}_{i,j}(k) | s_{ij}^k = 1, \vect{p}_i(k),\vect{X}_j(k) )$。则二元胞观测$\vect{z}_{ij}^k$的概率模型可进行如下分解：
\begin{equation} \label{eq. ACOTO_decomMea}
	\begin{aligned}
		& p(\vect{z}_{i,j}^k|\vect{p}_{i}^k,\vect{X}_j^k) = p(\vect{\vec{z}}_{i,j}^k,s_{ij}^k|\vect{p}_{i}^k,\vect{X}_j^k) \\
		&= p(\vect{\vec{z}}_{i,j}^k|\vect{p}_{i}^k,\vect{X}_j^k,s_{ij}^k = 1)  p(s_{ij}^k = 1|\vect{p}_{i}^k,\vect{X}_j^k)
	\end{aligned}
\end{equation}

\begin{table}[tbp]
	\bicaption[Tab. NotationsTab]{}{符号表达式的定义总览}{Table$\!$}{Collection of notations and definitions}
	\vspace{0.5em}\centering \wuhao
	\begin{tabular}{cc}
		\toprule[1.5pt]
		符号 & 定义 \\
		\midrule[1pt]
			$\vect{p}^k_i,\vect{W}_j$  & 第$i$个机器人在$k$时刻的状态，以及第$j$个锚节状态 \\
			$\vect{z}^{k}_{i,j}:= \{ \vect{\vec{z}}_{ij}^k, s_{ij}^k \}$  & 第$i$个节点和第$j$个节点在$k$时刻的测量 \\
			$\vect{u}^{k}_{i}$  & 第$i$个节点在$k$时刻的控制输入 \\
			$\vect{X}^k:=\{ \vect{p}^k_1,\cdots,\vect{p}^k_N \}$  & 所有机器人节点在$k$时刻状态的集合 \\
			$\vect{X}^{k_1:k_2}:=\{ \vect{X}^{k_1},\cdots,\vect{X}^{k_2} \}$  & 所有机器人节点在$k_1$和$k_2$时刻状态的集合 \\
			$\vect{Z}^{k}_{i}:=\{ \vect{z}_{i,j}^k | \forall v_j \in \vect{N}_i^k \} = \{ \vect{\vec{Z}}^k_i, \vect{N}_i^k \}$  & 第$i$个机器人节点在第$k$时刻所有测量的集合 \\
			$\mathcal{E}^{k}:= \vect{N}_1^k \bigcup \vect{N}_2^k \bigcup \cdots \bigcup \vect{N}_N^k $ & 机器人集群在第$k$时刻的通信测量拓扑集合 \\	
			$\vect{Z}^k,\vect{Z}^{k_1:k_2},\vect{U}^{k},\vect{U}^{k_1:k_2},\mathcal{E}^{k_1:k_2}$ & 与$\vect{X}^k$ 和 $\vect{X}^{k_1:k_2}$的定义相同\\		
			$\vect{W}:=\{ \vect{W}_1,\cdots,\vect{W}_M \}$ & 环境中所有锚节点的状态\\
			$[\vect{XW}]^k:=\{ \vect{X}^k, \vect{W} \}$ & 第$k$时刻环境中所有节点的状态 \\ 
			$\vect{X}_j^k \in \vect{[XW]}^k$	&  环境中第$j$个节点的状态(机器人或锚节点) \\
			$\mathcal{H}^{0:k}:=\{ \vect{Z}^{0:k}, \vect{U}^{0:k-1}, \vect{W} \}$ & 截止到$k$时刻的所有历史数据 \\
			$\mathcal{H}^{0:k+l|k}:=\{ \mathcal{H}^{0:k}, \vect{U}^{k:k+l-1} \}$ & 所有历史数据以及截至到第$l$个规划时刻的控制输入 \\
			$\vect{E}:=\{ \mathcal{E}^{k+1:k+L}_{iter} | \forall iter \in \mathbb{N}^+_{N_{con}} \}$  & $L$ 个规划期限内所有可能的通信观测拓扑集合 \\
		\bottomrule[1.5pt]
	\end{tabular}
\end{table}

将多智能体系统视作一个运动整体，表~\ref{Tab. NotationsTab}定义了该系统内部变量的联合向量形式. 因此，整个多智能体系统的后验概率密度可以表示为：
\begin{equation}
	p(\vect{X}^{0:k}|\vect{Z}^{0:k},\vect{U}^{0:k-1}),
\end{equation} 
称上式的后验概率密度为系统$0:k$时刻的信念状态(Belief State)。当给定一组关于机器人状态的先验分布$p(\vect{p}_i^0),\forall i \in \mathcal{V}_R$，则系统的信念，即后验概率密度函数可进一步分解为：
 \begin{equation} \label{eq. belief expansion at current step}
	\begin{aligned}
		&b(\vect{X}^{0:k}) = p(\vect{X}^{0:k}|\mathcal{H}^{0:k}) \\
		&= \prod_{i \in \mathcal{V}_R} p(\vect{p}_i^0) \prod_{t=1}^{k} \left[ p(\vect{p}_{i}^{t}|\vect{p}_i^{t-1},\vect{u}_i^{t-1}) \prod_{j\in \vect{N}_i^t} p(z_{i,j}^t|\vect{p}_i^t,\vect{X}_j^t) \right].
	\end{aligned}
\end{equation}

由于本文假设系统受到的所有干扰均为高斯白噪声模型，因此上述信念分布也符合一个高斯正太密度分布：
 \begin{equation} 
 	\begin{aligned}
 		b(\vect{X}^{0:k})  \sim \mathcal{N}(\vect{\bar{X}}^{0:k},\vect{\Sigma}_{\bar{X}^{0:k}}^k).
 	\end{aligned}	
\end{equation} 
式中$\vect{\bar{X}}^{0:k} = \left[ \vect{\bar{X}}^{0},\vect{\bar{X}}^{1},\cdots,\vect{\bar{X}}^{k} \right]^T$是对所有节点位置估计的均值向量（其中仅对应于智能体的状态需要估计，锚节点的位置状态是精确已知的），$\vect{\Sigma}_{\bar{X}^{0:k}}^k$表示在$k$时刻估计出的所有状态的协方差信息。

当给定一组未来$L$个时刻的控制输入$\vect{U}^{k:k+L-1}$，在给定的控制输入下，未来$L$时刻的先验信念分布为：
\begin{equation} \label{eq. ACOTO_priorBeliefGivenControl}
	\begin{aligned}
		b(\vect{X}^{0:k+L|k}) & = b(\vect{X}^{0:k}) \prod_{t = 1}^{L} p(\vect{X}^{k+t}| \vect{X}^{k+t-1}, \vect{U}^{k+t-1}) \\
		& = p(\vect{X}^{0:k+L} | \mathcal{H}^{0:k+L|k})
	\end{aligned}
\end{equation}

如图~\ref{fig. ACTCO_estimator}所示，状态估计问题就是根据已测量得到的数据和系统模型，求解一个符合后验概率密度分布的最优均值向量和协方差矩阵。典型的估计过程求解方法分为两类。一种是基于马尔可夫性质(Markov Property)的贝叶斯滤波方法，以著名的卡尔曼滤波、拓展卡尔曼滤波、粒子滤波等方法为代表。此类方法的特点是借助马尔可夫性质，边缘化过去时刻的系统状态，从而实现相邻两个时刻间状态的递归估计。另一类方法是基于优化过程的平滑估计方法，以非线性最小二乘、图优化等理论为典型代表。此类方法的特征是侧重于系统轨迹的估计（同时估计系统的历史状态）而不是仅仅估计系统在当前时刻的状态。后文将分别称上述两类估计方法为滤波引擎和平滑引擎，它们的在协同定位任务中的具体使用方法可参见附录~\ref{ch5sec. appendix}。
\begin{figure}[ht]
	\centering
	\includegraphics[width = 0.7\textwidth]{figures_ACTCO/estimator}
	\bicaption[fig. ACTCO_estimator]{}{状态估计问题的输入输出}{Fig.$\!$}{Scenairo of active and cooperative localization}\vspace{0em}
\end{figure}

\BiSubsection{信念空间规划框架}{arg2}
本小节详细给出信念空间规划的步骤。信念空间规划问题的目的是，在当前时刻$k$，寻找未来$L$个规划周期内系统中所有节点的控制输入$\vect{U}^{k:k+L-1}$，从而优化一个给定的目标函数(奖励函数/损失函数)。该目标函数的定义一般与系统在未来时刻状态的信念分布有关，例如，
\begin{equation} \label{eq. ACTCO_objective_function}
	J_{k}(\vect{U}^{k:k+L-1}) = \mathbb{E} \left[ \sum_{l=1}^{L} c^{l} \left( b(\vect{X}^{k+1:k+l}), \vect{U}^{k:k+l-1} \right)  \right],
\end{equation}
式中$\mathbb{E}[\cdot]$为取期望值的操作符。由于在规划时刻$k$，系统无法获取未来规划期限内的观测数据$\vect{Z}^{k+1:k+L}$，因此目标函数的需要考虑未来所有可能的观测数据，即对观测随机变量取期望。$b(\vect{X}^{k+1:k+l})$与式~\eqref{eq. ACTCO_MAP}的定义类似，为系统在未来$L$个规划周期内的状态的信念分布。$c^l(\cdot)$为与未来第$l$个规划时刻中系统状态有关的目标函数计算式。$c^l(\cdot)$的具体形式与实际的任务配置有关，但一般情况下，目标函数由如下三类指标所组成：
\begin{itemize}
	\item[1):] 与控制输入量有关的能耗指标$J^C_k$。该指标的目的通常是保证系统选取的控制输入满足一定的燃料消耗约束。该指标的计算通常与控制输入量的范数相关。
	\item[2):] 与高层任务完成度相关的任务指标$J^T_k$。该指标与系统接受的高层任务的形式有关，适用于确保系统完成一定的期望任务，例如驱使系统搜索某篇区域或实现特定轨迹跟踪等。
	\item[3):] 与系统自定位精度相关的定位指标$J^L_k$。该指标是路径规划与协同定位产生连结的重要环节。即在规划系统路径，考虑各种约束和任务时，将自定位精度作为优化指标的一部分，从而在保证约束和任务完成得同时，尽量低让系统内部形成优化得观测拓扑结构、获取更有效得测量，从而降低系统协同定位过程得自定位误差。
\end{itemize}

因此在确定了合适的目标函数后，系统的运动策略的确定方法为（此处以目标函数为损失函数为例）：
\begin{equation} \label{eq. ACTCO_original_motion_policy}
	\begin{aligned}
		\vect{U}^{k:k+L-1}_{\star} &= \{ \vect{U}^{k}_{\star},\vect{U}^{k+1}_{\star},...,\vect{U}^{k+L-1}_{\star} \} \\
		&= \mathop{\arg\min}_{\vect{U}^{k:k+L-1} } \quad J_{k}(\vect{U}^{k:k+L-1}).
	\end{aligned}
\end{equation}

\begin{figure}[ht]
	\centering
	\includegraphics[width = 0.8\textwidth]{figures_ACTCO/activePlanningFlowChart}
	\bicaption[fig. ACTCO_activePlanningFlowChart]{}{POMDP问题通用求解过程}{Fig.$\!$}{Scenairo of active and cooperative localization}\vspace{0em}
\end{figure}


上述信念空间的路径规划任务是一类典型的部分可观测马尔可夫决策过程(Partially Observable Markow Decision Process, POMDP)。
该问题具有极高的复杂度，至今仍不存在求取全局最优解的有效方法。因此，由于计算资源受限，实际案例通常采用与模型预测控制类似的优化过程，以获取局部最优解。如图~\ref{fig. ACTCO_activePlanningFlowChart}所示，该过程包括如下四个步骤：
\begin{itemize}
	\item[1):] 根据已获取的数据生成对当前环境和自身状态的估计（求解估计问题）。
	\item[2):] 生成有限个待评估的控制输入；
	\item[3):] 对上一步生成的每一个控制输入，预测在该输入下系统的未来行为，计算式~\eqref{eq. ACTCO_objective_function}所对应的目标函数值；
	\item[4):] 返回局部最优解。
\end{itemize}
以上程中，步骤1)的本质是求解系统当前状态的估计问题，根据已获取的观测数据，求解一组满足式~\eqref{eq. belief expansion at current step}中后验概率密度的最优正太分布；

步骤2)根据状态估计，结合顶层任务需求，生成满足主要任务需求的待评估控制输入。生成候选控制的方法主要有两类，分别为
\begin{itemize}
\item[1):] 基于梯度下降的连续空间生成方法；.
\item[2):] 基于概率抽样的离散空间生成方法（如RRT，PRM，$A^{\star}$等算法）
\end{itemize}

步骤3)以式~\eqref{eq. ACTCO_objective_function}的计算结果作为评价指标，衡量每一个候选控制输入的优劣。该过程式整个规划问题的核心，通常涉及对系统未来信念状态的预测。 

最后，步骤4）中判断是否需要返回局部最优解的典型依据一般有：
\begin{itemize}
	\item[1):] 采用离散的控制生成模块情况下，当超过最大循环次数$r_{max}$时，可终止。
	\item[2):] 采用梯度下降的控制生成模块的情况下，当相邻两个控制输入之间的目标函数变化小于特定条件时，可终止。
\end{itemize}

\BiSubsection{本章主要任务}{arg2}
本章的主要任务是：
\begin{itemize}
	\item[1):] 分析概率拓扑对信念空间规划过程的影响。
	\item[2):] 针对两类典型的求解引擎，。
\end{itemize}

\BiSection{概率拓扑对目标函数的影响}{arg2}

本小节将展开式~\eqref{eq. ACTCO_objective_function}目标函数中的取期望操作，分解目标函数的计算过程，并重点分析概率拓扑对目标函数的影响。首先，规划期限内的观测变量也包含实际观测数据和观测拓扑两部分，即$\vect{Z}^{k+1:k+L}=\{ \vect{\vec{Z}}^{k+1:k+L},\mathcal{E}^{k+1:k+L} \}$，因此可以将式~\eqref{eq. ACTCO_objective_function}中的取期望操作具化为积分操作，并使用全概率公式可得：
\begin{equation*} 
	\begin{aligned}
		& J_{k}(\vect{U}^{k:k+L-1}) =   \int_{\vect{\vec{Z}}^{k+1:k+L}} \int_{\mathcal{E}^{k+1:k+L}} p( \vect{\vec{Z}}^{k+1:k+L}, \mathcal{E}^{k+1:k+L} |\mathcal{H}^{0:k+L|k} ) \\
		& \left[ \sum_{l=1}^{L} c^{l} \left( p(\vect{X}^{0:k+l}|\mathcal{H}^{0:k+l|k}, \vect{\vec{Z}}^{k+1:k+l}, \mathcal{E}^{k+1:k+l}), \vect{U}^{k:k+l-1} \right)  \right]  \\
		& = \int_{\mathcal{E}^{k+1:k+L}} p( \mathcal{E}^{k+1:k+L} |\mathcal{H}^{0:k+L|k} )  \int_{\vect{\vec{Z}}^{k+1:k+L}}  p( \vect{\vec{Z}}^{k+1:k+L}  |\mathcal{H}^{0:k+L|k},\mathcal{E}^{k+1:k+L} ) \\
		& \left[ \sum_{l=1}^{L} c^{l} \left( p(\vect{X}^{0:k+l}|\mathcal{H}^{0:k+l|k}, \vect{\vec{Z}}^{k+1:k+l}, \mathcal{E}^{k+1:k+l}), \vect{U}^{k:k+l-1} \right)  \right] 
	\end{aligned}	
\end{equation*}
由于在规划期限内，观测、通信拓扑的分布是离散的，其值域$\vect{E}$的规模是有限的。给定集群的规模为$N$，环境中存在$M$个锚节点，规划期限为$L$，则未来规划期限内拓扑集合$\vect{E}$中最多可以包含$N_{con}$个元素，
$$N_{con} = 2^{N_s}, N_s = \left[ \frac{L(M+N)(M+N-1)}{2} \right].$$

目标函数可进一步转变为：
\begin{equation} \label{eq. ACTCO_objective function1}
	\begin{aligned}
		& J_{k}(\vect{U}^{k:k+L-1}) =  \sum_{\mathcal{E}^{k+1:k+L}_{iter} \in \vect{E}} \underbrace{ p( \mathcal{E}^{k+1:k+L}_{iter} |\mathcal{H}^{0:k+L|k} ) }_{A} \int_{\vect{\vec{Z}}^{k+1:k+L}}  \underbrace{ p( \vect{\vec{Z}}^{k+1:k+L}  |\mathcal{H}^{0:k+L|k},\mathcal{E}^{k+1:k+L}_{iter} ) }_{B} \\
		& \left[ \sum_{l=1}^{L} c^{l} \left( \underbrace{ p(\vect{X}^{0:k+l}|\mathcal{H}^{0:k+l|k}, \vect{\vec{Z}}^{k+1:k+l}, \mathcal{E}^{k+1:k+l}_{iter})}_{C}, \vect{U}^{k:k+l-1} \right)  \right] 
	\end{aligned}	
\end{equation}
因此，当给定一组待评估的候选控制${\vect{U}}^{k:k+L-1}$，其所对应的目标函数包含三部分：$A$为未来时刻不确定性拓扑的似然项；$B$为在给定观测拓扑前提下，观测传感器实际测量数据的似然；$C$为在给定观测拓扑和实际观测数据前提下，对联合状态向量后验概率密度分布的预测。下面分别对$A,B,C$三项进行详细分析：

1）\textbf{B-测量似然}$p( \vect{\vec{Z}}^{k+1:k+L}  |\mathcal{H}^{0:k+L|k},\mathcal{E}^{k+1:k+L}_{iter} )$:

首先分析测量似然项，由于该项并非本章所研究的重点，为了简化问题并突出概率拓扑的影响，本文将采用最大似然假设来计算测量似然项的取值，即
$$p( \vect{\vec{Z}}^{k+1:k+L}  |\mathcal{H}^{0:k+L|k},\mathcal{E}^{k+1:k+L}_{iter} ) = \left\lbrace \begin{array}{ll}
	1, & \quad \text{如果 } \vect{\vec{Z}}^{k+1:k+L} = \vect{\vec{Z}}^{k+1:k+L}_{ML} \\
	0, & \quad \text{否则}
\end{array} \right.
$$
因此，目标函数~\eqref{eq. ACTCO_objective function1}可转换为：
\begin{equation} \label{eq. ACTCO_objective function2}
	\begin{aligned}
		& J_{k}(\vect{U}^{k:k+L-1}) =  \sum_{\mathcal{E}^{k+1:k+L}_{iter} \in \vect{E}} \underbrace{ p( \mathcal{E}^{k+1:k+L}_{iter} |\mathcal{H}^{0:k+L|k} ) }_{A} \\
		& \left[ \sum_{l=1}^{L} c^{l} \left( \underbrace{ p(\vect{X}^{0:k+l}|\mathcal{H}^{0:k+l|k}, \vect{\vec{Z}}^{k+1:k+l}_{ML}, \mathcal{E}^{k+1:k+l}_{iter})}_{C}, \vect{U}^{k:k+l-1} \right)  \right] 
	\end{aligned}	
\end{equation}

依据最大似然假设，实际观测数据$\vect{\vec{Z}}^{k+1:k+L}_{ML}$可由式~\eqref{eq. ACOTO_priorBeliefGivenControl}中的先验信念分布生成。因此，在后文的具体算例中，需要首先调用附录~\ref{ch5sec. appendix}中的估计引擎，求解符合式~\eqref{eq. ACOTO_priorBeliefGivenControl}的高斯分布状态均值$\vect{\bar{X}}^{k+1:k+L|k}$，然后使用该状态均值和测量设备的观测模型生成无误差的最大似然观测数据$\vect{\vec{Z}}^{k+1:k+L}_{ML}$，每一组数据的存在与否受给定拓扑$\mathcal{E}^{k+1:k+L}_{iter}$的约束。

2） \textbf{C-未来状态的后验概率密度分布}$p(\vect{X}^{0:k+l}|\mathcal{H}^{0:k+l|k}, \vect{\vec{Z}}^{k+1:k+l}_{ML},\mathcal{E}^{k+1:k+l}_{iter})$：

该项表示在假定的通信观测拓扑$\mathcal{E}^{k+1:k+l}_{iter}$以及实际观测数据$\vect{\vec{Z}}^{k+1:k+l}_{ML}$下，系统在未来规划期限内状态向量的后验概率密度分布。由于本文采用高斯噪声假设，因此计算该后验概率密度分布等价于寻找一组高斯分布的状态均值$\vect{\bar{X}}^{0:k+l}$和分布协方差矩阵$\Sigma_{\bar{X}^{0:k+l}}$。
因此该项的计算与估计问题相似，可直接调用附录~\ref{ch5sec. appendix}中的滤波引擎和光滑引擎进行求解。

3）\textbf{A-拓扑似然}$p( \mathcal{E}^{k+1:k+L}_{iter} |\mathcal{H}^{0:k+L|k} )$:

针对拓扑似然概率项$p( \mathcal{E}^{k+1:k+L}_{iter} |\mathcal{H}^{0:k+L|k} )$，在系统在未来时刻的联合状态变量$\vect{X}^{k+1:k+L}$上再次运用全概率公式，式~\eqref{eq. ACTCO_objective function1}中的$A项$可作如下展开：
\begin{equation*}
	\begin{aligned}
		p( \mathcal{E}^{k+1:k+L}_{iter} &|\mathcal{H}^{0:k+L|k} )  = \int_{\vect{X}^{k+1:k+L}} p( \mathcal{E}^{k+1:k+L}_{iter}, \vect{X}^{k+1:k+L} |\mathcal{H}^{0:k+L|k} ) \\
		& = \int_{\vect{X}^{k+1:k+L}} p( \mathcal{E}^{k+1:k+L}_{iter} |\mathcal{H}^{0:k+L|k},  \vect{X}^{k+1:k+L} ) p(\vect{X}^{k+1:k+L} | \mathcal{H}^{0:k+L|k} )
	\end{aligned}
\end{equation*}
带入公式~\eqref{eq. ACOTO_priorBeliefGivenControl}中的先验信念，则上式对未来状态的积分可转换为使用先验信念来计算，即
\begin{equation}
	p( \mathcal{E}^{k+1:k+L}_{iter} |\mathcal{H}^{0:k+L|k} ) = p( \mathcal{E}^{k+1:k+L}_{iter} |\mathcal{H}^{0:k+L|k},  \vect{X}^{k+1:k+L|k} ) b( \vect{X}^{k+1:k+L|k} )
\end{equation}

本章假设未来网络拓扑集合$\mathcal{E}^{k+1:k+L}$中的任意两个元素在空间上和时间上都是互相独立的。传统的状态估计问题通常直接使用马尔可夫独立假设，即确定了系统的当前状态后，系统在未来时刻的行为与系统的过去状态无关。该假设认为当前时刻系统的状态是完备的，即过去时刻的任何信息，包括系统状态、控制输入以及观测数据等，对未来的系统行为不会产生影响。因此，马尔可夫独立假设保证了测量数据在时间上的相互独立性。另外，在某一时刻，各个节点在执行测量操作时，通常认为每个节点的测量操作是独立进行的，因此保证了测量数据在空间上的独立性。由此可以推断，在状态估计问题中，网络拓扑集合$\mathcal{E}^{0:k}$中的任意两个元素在时间上和空间上都是互相独立的。本章主要考虑规划问题。由于在规划时刻并不存在未来的观测数据，本章将沿用估计问题中拓扑独立的性质，假设未来时刻的网络拓扑集合$\mathcal{E}^{k+1:k+L}$中任意两个元素在空间上和时间上也都是互相独立的，即
\begin{itemize}
	\item[1):] 同一个时刻中的任意两条观测和通信连通的存在关是相互独立的，即
	\begin{equation} \label{eq. connectionSpatialIndependence}
		p(\mathcal{E}^{t}) = \prod_{i \in \mathcal{V}_R} \prod_{j \in \vect{N_i^k}} p(s_{i,j}^t), \forall  k+1 \le t \le k+L
	\end{equation}
	\item[2):] 任意两个时刻之间系统的拓扑关系是相互独立的，即
	\begin{equation} \label{eq. connectionTemporalIndependence}
		p(\mathcal{E}^{k+1:k+L}) = \prod_{t=k+1}^{k+L} p(\mathcal{E}^{t}).
	\end{equation}	
\end{itemize}
一般而言，在保证同一时刻各个测量操作之间不存在相互干扰的情况下，规划过程测量数据的空间独立性将依然成立。然而由于估计问题中测量数据之间在时间上的独立性是基于马尔可夫假设推导而出的，因此时间独立性在规划过程的成立与否将取决于马尔科夫假是否依然有效。规划期限内马尔可夫属性的合理性有待进一步研究。

由拓扑概率的时空独立假设，目标函数\eqref{eq. ACTCO_objective function1}中与网络拓扑相关的概率项$p( \mathcal{E}^{k+1:k+L} |\mathcal{H}^{0:k+L|k} )$可进一步展开为：
\begin{equation}
	p( \mathcal{E}^{k+1:k+L}_{iter} |\mathcal{H}^{0:k+L|k} ) = \prod_{s_{ij}^t \in \mathcal{E}^{k+1:k+L}} p(s_{i,j}^t | \vect{p}_i^t, \vect{X}_j^t)  p(\vect{p}_i^t, \vect{X}_j^t | \mathcal{H}^{0:k+L|k} )
\end{equation}
上式中$p(s_{i,j}^t | \vect{p}_i^t, \vect{X}_j^t)$表示当两个节点的分布$\vect{p}_i^t, \vect{X}_j^t$已知时，它们之间存在观测连通的概率。本章沿用前一章的圆盘通信观测模型假设，则每一个观测连通的概率均可采用算法~[TBD]进行计算。

\BiSubsection{目标函数的计算框架}{arg2}
至此，对概率拓扑的信念空间规划问题，其目标函数计算过程可以从式~\eqref{eq. ACTCO_objective function1}和式~\eqref{eq. ACTCO_objective function2}总结为如图~\ref{fig. ACTCO_ComputeObjective}所示的计算框架：

\begin{figure}[ht]
	\centering
	\includegraphics[width = 0.95\textwidth]{figures_ACTCO/computeObjective.jpg}
	\bicaption[fig. ACTCO_ComputeObjective]{}{目标函数计算过程}{Fig.$\!$}{Scenairo of active and cooperative localization}\vspace{0em}
\end{figure}

因此，为了评估一组候选的控制输入$\vect{U}^{k:k+L-1}$，需要计算全部可能的拓扑现实下，目标函数的取值，然后将拓扑似然作为权值，对全部目标函数值进行权值累加，即可得到与$\vect{U}^{k:k+L-1}$对应的最终目标函数值$J_k(\vect{U}^{k:k+L-1})$。针对每一个拓扑现实，都需要依照式~\eqref{eq. ACTCO_objective function1}中分解的$A,B,C$三项分别进行计算。

本章的主要任务是在信念空间规划问题框架下，分别针对滤波引擎和平滑引擎两类估计问题的求解算法，设计基于概率拓扑的目标函数计算方法。

\BiSection{基于概率拓扑的目标函数计算}{arg2}



\BiSubsection{给定拓扑的预测过程——滤波引擎}{arg2}

\BiSubsection{给定拓扑的预测过程——平滑引擎}{arg2}

\BiSubsection{目标函数计算算法}{arg2}


\BiSection{附录}{Appendix} \label{ch5sec. appendix}

协同定位的目的是通过处理协同系统中收集到的通信和观测数据，估计系统中所有节点的位姿信息。因此协同定位任务是典型的状态估计问题。目前在定位任务的背景下，此类状态估计问题的求解方法主要有两种，即
\begin{itemize}
	\item[1):] 基于马尔可夫假设的递归贝叶斯滤波方法。此类方法假设系统的时序状态之间具有相互独立的“无记忆”马尔可夫性质，即系统在下一时刻的状态只能由当前状态和控制输入决定，与过去和未来的状态无关。因此，此类滤波方法可以使用贝叶斯定理和全概率公式，通过递归的方式估计每一时刻的系统状态。在每一次递归过程中，由于前一时刻的系统状态已知，因此估计方法仅需处理并且得到当前时刻的状态估计值。此类方法的典型代表是卡尔曼滤波，拓展卡尔曼滤波以及粒子滤波等方法。
	\item[2):] 基于非线性最小二乘的最大后验概率平滑方法。与贝叶斯滤波不同的是，此类方法将系统现在和过去时刻所有的状态共同作为估计量，根据所有历史测量数据，同时估计系统在现在和过去时刻的全部轨迹。一类典型的求解过程是将历史数据中的每一次测量转换为信念网络 (belief network) 中的一个节点因子 (factor), 然后通过优化整个信念网络的最大后验概率，得出系统历史轨迹整体的平滑估计。当系统模型采用高斯噪声时，求解最大后验概率的过程可以转变为求解一类非线性最小二乘问题。
\end{itemize}

状态估计问题的输入输出过程如图~\ref{fig. ACTCO_estimator}所示。为了更好地区分上述两种方法，后文将它们称为状态估计问题的求解引擎，分别表述为“滤波引擎”和“平滑引擎”。本节将沿用前一章的任务配置以及相应的数学表示符号，简要地介绍以上两类状态估计方法在协同定位任务中的应用。

多智能体协同定位问题是在已知系统运动模型和历史测量数据的基础上，寻找优化的$\vect{\bar{X}}^{0:k}$以及$\vect{\Sigma}^k_{\vect{\bar{X}}^{0:k}}$，以匹配如下的联合信念(belief, 即后验概率密度函数)：
\begin{equation} \label{eq. ACTCO_MAP}
	\begin{aligned}
		b(\vect{X}^{0:k}) &= p(\vect{X}^{0:k}|\vect{Z}^{0:k},\vect{U}^{0:k-1}) \\
		&= \prod_{i \in \mathcal{V}_R} p(\vect{p}_i^0) \prod_{t=1}^{k} \left[ p(\vect{p}_{i}^{t}|\vect{p}_i^{t-1},\vect{u}_i^{t-1}) \prod_{j\in \vect{N}_i^k} p(z_{i,j}^t|\vect{X}_i^t,\vect{X}_j^t) \right].\\
		& \sim \mathcal{N}(\vect{\bar{X}}^{0:k},\vect{\Sigma}_{\bar{X}^{0:k}}^k).
	\end{aligned}	
\end{equation} 
式中$\vect{X}_i^t,\vect{X}_j^t$为$t$时刻，系统内存在相对测量链的第$i$和第$j$个节点的状态值。$p(\vect{p}_i^0)$为节点在初始时刻的先验概率，$p(\vect{p}_{i}^{t}|\vect{p}_i^{t-1},\vect{u}_i^{t-1})$为运动模型~\eqref{eq. PROB_motionModel}中表示的状态转移概率，$p(z_{i,j}^t|\vect{X}_i^t,\vect{X}_j^t)$为观测模型~\eqref{eq. PROB_observationModel}中表示的观测数据的置信概率。

\BiSubsection{基于拓展卡尔曼滤波的协同定位}{Cooperative localization}

本节以中心化的拓展卡尔曼滤波方法（Extended Kalman Filter, EKF）为例，介绍贝叶斯滤波方法在多智能体系统协同定位任务中的应用。当存在时变的测量通信拓扑约束时，分析滤波方法用于预测系统未来状态行为的局限和难点。其他类型的非线性递归滤波方法，如无迹卡尔曼滤波，粒子滤波方法等，也可以进行类似地分析。

使用滤波过程中通用的数学表达形式，以$\vect{\bar{X} }(k|k-1)$和$\vect{\Sigma}(k|k-1)$表示系统对$k$时刻所有智能体位置状态的先验估计，用$\vect{\bar{X}}(k|k)$和$\vect{\Sigma}(k|k)$表示后验估计。
则对式~\eqref{eq. ACTCO_MAP}所示的最大后验概率密度，采用EKF进行状态估计，从$k-1$到$k$时刻的预测过程为：
\begin{align}
	\vect{\bar{X}}(k|k-1) &= \vect{f}[\vect{\bar{X}}(k-1|k-1),\vect{U}(k-1)],\\
	\vect{\Sigma}(k|k-1) &= \vect{F}(k|k-1)\vect{\Sigma}(k-1|k-1)\vect{F}(k|k-1)^T + \vect{R}(k),
\end{align} 
式中函数$\vect{f}$为所有智能体状态转移函数\eqref{eq. PROB_motionModel}组成的函数集合，即$\vect{f}=\{ f_i | \forall i \in \mathcal{V}_R \}$。 状态转移矩阵 $\vect{F}(k|k-1)$ 为函数$\vect{f}$ 在先验位置 $\vect{\bar{X}}(k|k-1)$ 处的雅可比矩阵，即
$$\vect{F}(k|k-1)=\frac{\partial \vect{f}}{\partial \vect{X}}\lvert_{\vect{X}=\vect{\bar{X}}(k|k-1)},$$ 
且$\vect{R} = diag([\vect{R}_1,\cdots,\vect{R}_N]^T)$为所有智能体运动过程的联合噪音协方差矩阵。

当第$k$时刻所有智能体之间执行了相对测量之后，得到所有邻接节点之间的相对测量数据$\vect{Z}(k)=\{ \vect{z}_{i,j}(k) | \forall i,j \in \mathcal{V}, \forall s_{i,j}(k) \in \mathcal{E}(k), i\neq j \}$，则EKF的测量更新过程为：
\begin{align}
	\vect{\bar{X}}(k|k) &= \vect{\bar{X}}(k|k-1) + \vect{K}(k) [ \vect{Z}(k) - \vect{h}^k(\vect{\bar{X}(k|k-1)}) ] \label{eq. ACTCO_residual_error}\\
	\vect{\Sigma}(k|k) &= [\vect{I} - \vect{K}(k)\vect{H}(k)]\vect{\Sigma}(k|k-1)
\end{align}
式中函数$\vect{h}^k$为$k$时刻所有有效的相对测量所组成的联合观测函数，即$\vect{h}^k = \{ h^k_{i,j} | \forall \vect{z}_{i,j}(k) \in \vect{Z}(k) \}$，$h_{i,j}$为第$i$个节点和第$j$个节点的相对观测\eqref{eq. PROB_observationModel}。其中$\vect{K}(k)$为卡尔曼滤波增益矩阵：
$$\vect{K}(k) = \vect{\Sigma}(k|k-1)\vect{H}^T(k)\left[ \vect{H}(k)\vect{\Sigma}(k|k-1)\vect{H}^T(k) + \vect{Q}(k) \right]^{-1},$$
$\vect{H}(k)$为联合测量函数$\vect{h}$在预测的先验状态值$\vect{\bar{X}}(k|k-1)$处的线性化雅可比矩阵：
$$\vect{H}(k) = \frac{\partial \vect{h}}{\partial \vect{X}} |_{\vect{X} = \vect{\bar{X}}(k|k-1)}.$$ 其中联合观测的噪音协方差矩阵为$\vect{Q} = diag([\vect{Q}_{i,j}]),\forall \vect{z}_{i,j}\in \vect{Z}(k)$。

以上为标准的中心化EKF在协同定位任务中对系统状态的估计过程。该流程通过递归调用，可以估计任意$t-1$到$t$时刻系统状态的转移，$\forall t < k$。连续调用$k$次即可获得从$0$时刻到$k$时刻所有节点历史轨迹的估计值。

\BiSubsection{基于最大后验概率平滑估计的协同定位}{arg2}
最大后验概率平滑估计是寻找合适的历史轨迹$\vect{X}^{0:k}$，使得下面的后验概率最大化，
\begin{equation*} 
	\begin{aligned}
		\vect{\bar{X}}^{0:k} = & \mathop{\arg\max}\limits_{\vect{{X}}^{0:k}} b(\vect{{X}}^{0:k}) = \mathop{\arg\min}\limits_{\vect{{X}}^{0:k}} -\log b(\vect{{X}}^{0:k}) \\
		& = \mathop{\arg\min}\limits_{\vect{{X}}^{0:k}} -\log p(\vect{X}^{0:k}|\vect{Z}^{0:K},\vect{U}^{0:k-1})		
	\end{aligned}
\end{equation*}

将式~\eqref{eq. ACTCO_MAP}中后验概率密度的展开式带入可得，
\begin{equation} \label{eq. ACTCO_smoothing_expanded_MAP}
	\begin{aligned}
		\vect{\bar{X}}^{0:k} &= \mathop{\arg\min}\limits_{\vect{{X}}^{0:k}}  \sum_{t=1}^{k}\sum_{i \in \mathcal{V}}|| \vect{p}_i^{t} - f_i(\vect{p}_i^{t-1},\vect{u}_i^{t-1},0) ||^2_{\vect{R}_i} \\
		&+ \sum_{t=1}^{k}\sum_{i \in \mathcal{V}}\sum_{j\in\vect{{N}}^{t}_i}||z_{i,j}^{t} - h_{ij}(\vect{X}_i^{t},\vect{X}_j^{t},0)||^2_{{\vect{Q}}_{ij}},
	\end{aligned}
\end{equation}
式中采用标准的马氏范数表达式，即$||\vect{e}||^2_{\Sigma} = \vect{e}^T\vect{\Sigma}^{-1}\vect{e}$；$\vect{R}_i,\vect{Q}_{i,j}$为节点运动过程和测量过程的噪音协方差矩阵。在估计问题中，求解上述最小优化问题的典型方法为采用高斯-牛顿(Gauss-Newton, GN) 迭代策略。 GN迭代过程将上式在先验估计 $\vect{\bar{X}}^{0:k}_{(0)}$ 处进行线性化，从而得到一步向量更新$\Delta \vect{\bar{X}}^{0:k}_{(0)}$，然后通过$\vect{\bar{X}}^{0:k}_{(1)} \leftarrow \vect{\bar{X}}^{0:k}_{(0)} + \vect{\bar{X}}^{0:k}_{(0)}$对状态估计值进行更新。该过程需要迭代多次直至收敛。下面以该过程的第一次迭代内容为例，将GN迭代方法详细介绍如下。

GN迭代的重点为将式~\eqref{eq. ACTCO_smoothing_expanded_MAP}中的每一个累加因子在线性点处进行线性化，然后通过最小二乘法则推导出状态向量的一步更新。在第一次迭代时，需要对线性点进行初始化，一类典型的初始化方法是使用已知的初始节点位置$\vect{X}^0$，每一时刻的控制输入$\vect{U}^{0:k-1}$,和节点的运动模型~\eqref{eq. PROB_motionModel}，在无噪音的理想环境下递推出一组系统所有节点轨迹的初始估计，即令
\begin{equation*}
	\vect{\bar{X}}^{0:k}_{(0)} = \begin{bmatrix}
		\vect{X}^0_{(0)} \\
		\vect{X}^1_{(0)} \\
		\vdots \\
		\vect{X}^k_{(0)} \\
		\vect{W}
	\end{bmatrix} = \begin{bmatrix}
		\vect{X}^0_{(0)} \\
		\vect{f}_{1:N}(\vect{X}^0_{(0)}, \vect{U}^{0},0) \\
		\vdots \\
		\vect{f}_{1:N}(\vect{X}^{k-1}_{(0)}, \vect{U}^{k-1},0) \\
		\vect{W}
	\end{bmatrix}
\end{equation*}
式中$\vect{f}_{1:N}:= \{ f_i, \forall i \in \mathcal{V}_R \}$，为所有节点在同一时刻的运动模型组成的向量。

使用上述位置对式~\eqref{eq. ACTCO_smoothing_expanded_MAP}进行线性化，则式~\eqref{eq. ACTCO_smoothing_expanded_MAP}可转变为：
\begin{equation} \label{eq. ACTCO_smooth_MAP_after_linearization}
	\begin{aligned}
		\qquad \mathop{\arg\min}\limits_{\vect{{X}}^{0:k}} 
		&\sum_{t=1}^{L} \sum_{i=1}^{N} \sum_{j\in\vect{\tilde{N}}^{t}_i} ||\vect{H}_{ij}^{t} \Delta \vect{X}_{i,j}^{t} - \bar{\vect{b}}_{ij,h}^{t} ||^2_{\vect{{Q}}_{ij}} \\
		&+ \sum_{t=1}^{L}\sum_{i=1}^{N} ||\Delta \vect{p}^{t}_i - \vect{F}^{t-1}_i\Delta \vect{p}_i^{t-1} - \bar{\vect{b}}_{i,f}^{t}||^2_{\vect{R}_i},
	\end{aligned}
\end{equation}
式中$\Delta \vect{X}_{i,j}^{t}$为稀疏的、按照对应节点分块的向量，其中仅对应于第$i$和第$j$个节点的向量块上存在$\Delta \vect{p}^{t}_j$ 和 $\Delta \vect{p}^{t}_j$，其余的向量块均为零。式中的矩阵$\vect{H}_{ij}^{t}$为在$t$时刻第$i$和第$j$个节点之间测量函数在线性点处的线性雅可比矩阵，$\vect{F}^{t}_i$为第$i$个节点在第$t$时刻运动模型在线性点处的线性雅可比矩阵，即
\begin{align*} 
	\vect{F}^{t-1}_i &= \frac{\partial {f}_i^{t-1}}{\partial \vect{X}} |_{\vect{X} = \vect{\bar{X}}^{0:k}_{(0)}} \\
	\vect{H}_{ij}^{t} &= \frac{\partial {h}_{ij}^{t}}{\partial \vect{X}} |_{\vect{X} = \vect{\bar{X}}^{0:k}_{(0)}} 
\end{align*}
另外式~\eqref{eq. ACTCO_smooth_MAP_after_linearization}中的向量$\bar{\vect{b}}_{ij,h}^{t}$和$\bar{\vect{b}}_{i,f}^{t}$定义如下：
\begin{align}
	\bar{\vect{b}}_{i,f}^{t} &= f_i^{t-1}(\vect{\bar{p}}_i^{t-1},\vect{u}_i^{t-1},0) - \vect{\bar{p}}_i^{t}, \\
	\bar{\vect{b}}_{ij,h}^{t} &= z_{i,j}^{t} - h_{ij}^{k+t}(\vect{\bar{p}}_i^{t},\vect{p}_j^{t},0), \label{eq. ACTCO_measurement_residual_smoothing}
\end{align}
式中$\vect{\bar{p}}_i^t$为线性点$\vect{\bar{X}}^{0:k}_{(0)}$中第$t$时刻的状态向量$\vect{\bar{X}}^{t}_{(0)}$里对应于第$i$个节点的状态值。由于第一次迭代中，线性点选取的特殊性，因此此处的$\bar{\vect{b}}_{i,f}^{t} = \vect{0}$是零向量，然而当迭代步数增加，线性点将不再是仅仅依据运动模型递推而来的先验状态，因此在后续的迭代中，$\bar{\vect{b}}_{i,f}^{t} $将不再是零向量。

将式~\eqref{eq. ACTCO_smooth_MAP_after_linearization}中每一个累加因子都整合为一个针对一步更新向量$\Delta \vect{\bar{X}}^{0:k}_{(0)}$的紧致形式，可得
\begin{equation} \label{eq. ACTCO_MAP_compact_matrix_form}
	|| \vect{A}^{0:k} \Delta \vect{X}^{0:k}_{(0)} - \vect{\bar{b}}^{0:k}(\vect{U}^{0:k-1},\vect{Z}^{0:k})||^2,
\end{equation}
原式~\eqref{eq. ACTCO_smooth_MAP_after_linearization}中的协方差矩阵被分解并被乘到了范数里面，即$||\vect{e}||_{\vect{\Sigma}}^2 = || \vect{\Sigma}^{-\frac{1}{2}} \vect{e} ||^2$。其他新引入的向量表达式为
\begin{equation} \label{eq. smoothing struture of A and b}
	\vect{A}^{0:k} = \left[ \begin{matrix}
		\vect{F}^{0:k} \\
		\vect{H}^{0:k}
	\end{matrix} \right],\qquad	
	\bar{\vect{b}}^{0:k} =\left[  \begin{matrix}
		\bar{b}_{i,f}^{t}, \forall i \in \mathcal{V}_R, \forall t \le k\\
		\bar{b}_{ij,h}^{k+t}, \forall i,j \in \mathcal{V}_R, \forall t \le k
	\end{matrix} \right].
\end{equation}
式中$\vect{F}^{0:k}$和$\vect{H}^{0:k}$为按照$\Delta \vect{\bar{X}}^{0:k}_{(0)}$向量中每个节点位置顺序，合理排列所有的运动过程线性雅可比矩阵$\vect{R}_i^{-\frac{1}{2}} \left[ \cdots, -\vect{F}_i^{t}, \cdots, \vect{I},\cdots \right], \forall i \in \mathcal{V}_R, \forall t \le k$ 以及所有测量过程的线性雅可比矩阵$\vect{Q}_{i,j}^{-\frac{1}{2}} \vect{H}_{i,j}^t,\forall i,j \in \mathcal{V}_R, \forall t \le k$，所形成的联合向量。

至此，针对紧致形式~\eqref{eq. ACTCO_MAP_compact_matrix_form}，可依据摩尔-彭斯(Moore-Pense)伪逆计算出一步更新向量$\vect{X}^{0:k}_{0}$,
\begin{equation}
	\Delta \vect{\bar{X}}^{0:k}_{(0)} = \left( (\vect{A}^{0:k})^T\vect{A}^{0:k} \right)^{-1}(\vect{A}^{0:k})^T \bar{\vect{b}}^{0:k},
\end{equation}
则初始的估计轨迹$\vect{X}^{0:k}_{0}$可更新为
\begin{equation}
	\vect{\bar{X}}^{0:k}_{(1)} = \vect{\bar{X}}^{0:k}_{(0)} + \Delta \vect{\bar{X}}^{0:k+L}_{(0)},
\end{equation}
以及当前估计值的估计协方差矩阵可按照如下公式计算：
\begin{equation} 
	\vect{\Sigma}_{\bar{\vect{X}}^{0:k}}^{(1)} = \left( (\vect{A}^{0:k})^T\vect{A}^{0:k} \right)^{-1}.
\end{equation} 

以上即为GN迭代方法的一次更新过程，此过程一般需要迭代多次，直至系统历史轨迹的估计值收敛到稳定值为止。
